#include "outline/bezier_build.h"
//#include "gslmath/multifit.h"
#include "outline/fast_bspline_adjust.h"
#include <limits>

namespace outline {

template <typename T, typename R>
bezier_builder<T, R>::bezier_builder()
{
	//method = simple_explicit_bezier_build;
	//max_iterations = 150;

	// optimized for simplify_using_bezier functions
	method = pull_to_polyline_build;
    auto_refit = true;
	max_iterations = 10;
	stop_mean_discrepancy = 0;
	clear();
}

template <typename T, typename R>
uint bezier_builder<T, R>::enough_points() const
{
	switch (method) {
	case pull_to_points_build:
	case pull_to_points_free_ends_build:
	//case simple_implicit_bezier_build:
	case pull_to_polyline_build:
	case reverse_build:
	case arkan_build:
		return 4;
	//case newton_bezier_build:
	//	return 6;
	};
	assert(false);
	return 0;
}

template <typename T, typename R>
void bezier_builder<T, R>::clear()
{ 
	parameters_.clear(); 
	chisq_ = std::numeric_limits<real_type>::max();
	total_iterations_ = 0;
}

template <typename T, typename R>
typename bezier_builder<T, R>::bezier_type const & bezier_builder<T, R>::build(
	nodes_type const & polyline)
{
	samples.clear();

	init(polyline);

	switch (method) {
	case pull_to_points_build:
	case pull_to_points_free_ends_build:
	//case simple_implicit_bezier_build:
		return simple_build(polyline);
	//case newton_bezier_build:
	//	return newton_build(polyline);
	case pull_to_polyline_build:
		return pull_to_polyline(polyline);
	case reverse_build:
		return build_reverse(polyline);
	case arkan_build:
		return build_arkan(polyline);
	};
	assert(false);
	return controls_;
}

template <typename T, typename R>
typename bezier_builder<T, R>::bezier_type const & bezier_builder<T, R>::simple_build(
	nodes_type const & polyline)
{
	for (uint iter = 0; iter < max_iterations; ++iter) 
	{
		//real_type chisq_before = chisq_;

		parameters_to_controls(polyline);
		//if (method == pull_to_points_build)
			controls_to_parameters_explicit(polyline);
		//else
		//	controls_to_parameters_implicit(polyline);
		++total_iterations_;

		if (/*chisq_ >= chisq_before ||*/ precision_reached())
			return controls_;
	}
	return controls_;
}

/*template <typename T, typename R>
typename bezier_builder<T, R>::bezier_type const & bezier_builder<T, R>::newton_build(
	nodes_type const & polyline)
{
	for (uint iter = 0; iter < max_iterations; ++iter) 
	{
		real_type chisq_before = chisq_;

		if (!newton_iter(polyline))
			return controls_;
		++total_iterations_;

		if (chisq_ >= chisq_before)
			return controls_;
	}
	return controls_;
}*/

template <typename T, typename R>
void bezier_builder<T, R>::init(nodes_type const & polyline)
{
	uint s = polyline.size();
	assert (s >= 4);
	uint old_size = parameters_.size();
	if (old_size > s)
		assert (false); //you should use clear() method first
	if (old_size == s) {
		//no init
		//assert (controls_.start() == polyline.front());
		//assert (controls_.finish() == polyline.back());
		return;
	}
	if (old_size == 0 || auto_refit) {
		//cold init
		total_iterations_ = 0;
		poles.assign(polyline.begin(), polyline.end());

		//controls_.line(polyline.front(), polyline.back());
		controls_.start() = polyline[0];
		controls_.start_control() = polyline[s/3];
		controls_.finish_control() = polyline[s*2/3];
		controls_.finish() = polyline[s-1];

		parameters_.resize(s);
		real_type d = real_type(1) / (s-1);
		for (uint i = 0; i < s; ++i)
			parameters_[i] = d*i;
		return;
	}
	assert (old_size < s);

	total_iterations_ = 0;
	assert(controls_.start() == polyline.front());
	//don't change control points
	controls_.finish() = polyline.back();

	real_type shrink = real_type(old_size-1) / (s-1);
	uint i;
	for (i = 1; i < old_size; ++i)
		parameters_[i] *= shrink;

	parameters_.resize(s);
	real_type d = real_type(1) / (s-1);
	for (; i < s; ++i)
		parameters_[i] = d*i;
}

template <typename T, typename R>
void bezier_builder<T, R>::controls_to_parameters_explicit(nodes_type const & polyline)
{
	assert (method == pull_to_points_build || method == pull_to_points_free_ends_build);
	uint n = parameters_.size();
	assert (n >= 4);
	uint i;
	for (i = 1; i < n-1; ++i) {
		real_type & t = parameters_[i];
		real_point_type value, deriv;
		controls_.value_n_deriv(t, &value, &deriv);
		t = scalar_product(deriv, real_point_type(polyline[i]) - value + t * deriv) / square(deriv);
	}

	//correction step: parameters should remain in the same order
	/*doesn't work :(
	assert(parameters_[0] == 0);
	assert(parameters_[n-1] == 1);
	for (i = 1; i < n-1; ++i) {
		real_type const & p = parameters_[i-1];
		real_type & t = parameters_[i];
		real_type const & n = parameters_[i+1];
		if (t <= p || t >= n)
			t = real_type(0.5) * (p + n);
	}*/
}

/*template <typename T, typename R>
void bezier_builder<T, R>::controls_to_parameters_implicit(nodes_type const & polyline)
{
	assert (method == simple_implicit_bezier_build);
	uint n = parameters_.size();
	assert (n >= 4);
	uint i;
	for (i = 1; i < n-1; ++i) {
		real_type & t = parameters_[i];
		real_type t2 = t*t;
		real_type t3 = t2*t;

		/* 1) worse than explicit: f(x_k+1) + f'(x_k+1)(x_k+1 - x_k) = r
		real_point_type s =
			(1 + 3*t -  9*t2 +  5*t3) * controls_.p[0] +
			(  - 3*t + 18*t2 - 15*t3) * controls_.p[1] +
			(          -9*t2 + 15*t3) * controls_.p[2] +
			(                  -5*t3) * controls_.p[3];

		real_point_type q =
			(-6 + 12*t -  6*t2) * controls_.p[0] +
			( 6 - 24*t + 18*t2) * controls_.p[1] +
			(     12*t - 18*t2) * controls_.p[2] +
			(             6*t2) * controls_.p[3];

		t = scalar_product(q, real_point_type(polyline[i]) - s) / square(q);*/

		/* 2) same as explicit: f(x_k) + f'(x_k+1)(x_k+1 - x_k) = r
		real_point_type d1 = controls_.p[1]-controls_.p[0];
		real_point_type d2 = controls_.p[2]-controls_.p[1];
		real_point_type d3 = controls_.p[3]-controls_.p[2];

		real_point_type s = 3*(
			(-t + 2*t2 -   t3) * d1 +
			(   - 2*t2 + 2*t3) * d2 +
			(          -   t3) * d3);

		real_point_type q = 3*(
			(1 - 2*t +   t2) * d1 +
			(    2*t - 2*t2) * d2 +
			(            t2) * d3);

		t = scalar_product(q, real_point_type(polyline[i]) - controls_.value(t) - s) / square(q);*/

		//3) also the same as explicit: f(x_k+1) = r
/*		real_point_type s =
			(1 - 3*t2 + 2*t3) * controls_.p[0] +
			(    6*t2 - 6*t3) * controls_.p[1] +
			(  - 3*t2 + 6*t3) * controls_.p[2] +
			(         - 2*t3) * controls_.p[3];

		real_point_type q =
			(-3 +  6*t - 3*t2) * controls_.p[0] +
			( 3 - 12*t + 9*t2) * controls_.p[1] +
			(      6*t - 9*t2) * controls_.p[2] +
			(            3*t2) * controls_.p[3];

		t = scalar_product(q, real_point_type(polyline[i]) - s) / square(q);
	}
}*/

template <typename T, typename R>
void bezier_builder<T, R>::parameters_to_controls(nodes_type const & polyline)
{
	if (method == pull_to_points_free_ends_build)
		chisq_ = controls_.best_fit_free_ends(polyline, parameters_);
	else
		chisq_ = controls_.best_fit(polyline, parameters_);
}

/*template <class R>
struct bezier_point {
	point<R> value, deriv;
	R r1, r2;
};

template <typename T, typename R>
bool bezier_builder<T, R>::newton_iter(nodes_type const & polyline)
{
	uint n = parameters_.size();
	assert (n >= 6);
	typedef bezier_point<real_type> bpoint_type;
	std::vector<bpoint_type> bpoints(n-2);

	multifit fit(4, n-2);

	chisq_ = 0;
	uint i;
	for (i = 1; i < n-1; ++i) {
		bpoint_type & b = bpoints[i-1];
		controls_.value_n_deriv(parameters_[i], &b.value, &b.deriv, &b.r1, &b.r2);
		fit.setX(i - 1, 0, -b.r1 * b.deriv.y);
		fit.setX(i - 1, 1, b.r1 * b.deriv.x);
		fit.setX(i - 1, 2, -b.r2 * b.deriv.y);		
		fit.setX(i - 1, 3, b.r2 * b.deriv.x);
		fit.sety(i - 1, outer_product(b.deriv,
			real_point_type(polyline[i]) - b.value + b.r1 * controls_.p[1] + b.r2 * controls_.p[2]));
		chisq_ += square(b.value - real_point_type(polyline[i]));
	}
	if (precision_reached())
		return false;

	fit.prepare();
	fit.run();

	real_point_type p1(fit.c(0), fit.c(1));
	real_point_type p2(fit.c(2), fit.c(3));

	//adjust parameters
	for (i = 1; i < n-1; ++i) {
		bpoint_type const & b = bpoints[i-1];
		parameters_[i] += scalar_product(b.deriv,
			real_point_type(polyline[i]) - b.value + b.r1 * (controls_.p[1] - p1) + b.r2 * (controls_.p[2] - p2)) / square(b.deriv);
	}

	controls_.start_control() = p1;
	controls_.finish_control() = p2;

	return true;
}*/

template <typename T, typename R>
void bezier_builder<T, R>::lengths(real_vector_type & l) const
{
	l = parameters_;
	controls_.parameter_to_length(l, 100); //why 100? if not 100 then how much?
}

template <typename T, typename R>
typename bezier_builder<T, R>::bezier_type const & bezier_builder<T, R>::pull_to_polyline(nodes_type const & polyline)
{
	assert (method == pull_to_polyline_build);
	real_vector_type lens(polyline.size());
	length(polyline, lens, true);

	if (total_iterations_ == 0)
		parameters_ = lens;

	for (uint iter = 0; iter < max_iterations; ++iter) {
		parameters_to_controls(polyline);

		//controls to parameters
		parameters_ = lens;
		controls_.length_to_parameter(parameters_, 100); //why 100? if not 100 then how much?

		++total_iterations_;
		if (precision_reached())
			return controls_;
	}
	return controls_;
}

enum bezier_pull {
	to_node,
	to_segm
};

// reverse build

struct bpoint {
	bezier_pull pull;
	unsigned i; //point index (start point of segment)
};

template <typename T, typename R>
bool bezier_builder<T, R>::intersect(real_type & a, vector2 const & p, vector2 const & d, vector2 const & b, vector2 const & n)
{
	real_type denom = outer_product(n, d);
	if (fabs(denom) < std::numeric_limits<real_type>::epsilon())
		return false;
	a = outer_product(n, b - p) / denom;
	return true;
}

template <typename T, typename R>
typename bezier_builder<T, R>::bezier_type const & bezier_builder<T, R>::build_reverse(nodes_type const & polyline)
{
	typedef std::vector<bpoint> bpoint_vector;
	uint n = polyline.size();
	bpoint_vector bvec(n);
	//real_type dt = real_type(1) / (n-1);
	uint i;
	for (i = 0; i < n; ++i) {
		//parameters_[i] = i * dt;
		bpoint & b = bvec[i];
		b.pull = to_node;
		b.i = i;
	}
	//parameters_[0] = 0;
	//parameters_[n-1] = 1;

	assert(poles.size() == n);

	for (uint iter = 0; iter < max_iterations; ++iter) 
	{
		chisq_ = controls_.best_fit(poles, parameters_);

		//controls to parameters
		for (i = 1; i < n-1; ++i) {
			real_point_type pos, deriv;
			controls_.value_n_deriv(parameters_[i], &pos, &deriv);
			real_point_type norm(deriv.y, -deriv.x);
			bpoint & b = bvec[i];
			if (b.pull == to_node) {
				bool may_go_backward = false, may_go_forward = false;
				real_type a_backward, a_forward;
				real_point_type d_backward, d_forward;
				real_point_type pole_backward, pole_forward;
				if (b.i > 0) {
					d_backward = real_point_type(polyline[b.i-1]) - real_point_type(polyline[b.i]);
					may_go_backward = intersect(a_backward, 
						polyline[b.i], d_backward, pos, norm)
						&& a_backward > 0;
					if (may_go_backward)
						pole_backward = real_point_type(polyline[b.i]) + a_backward * d_backward;
				}
				if (b.i < n-1) {
					d_forward = real_point_type(polyline[b.i+1]) - real_point_type(polyline[b.i]);
					may_go_forward = intersect(a_forward, 
						polyline[b.i], d_forward, pos, norm)
						&& a_forward > 0;
					if (may_go_forward)
						pole_forward = real_point_type(polyline[b.i]) + a_forward * d_forward;
				}
				if (may_go_backward && may_go_forward) {
					//choose the nearest point
					if (square(pole_backward - pos) > square(pole_forward - pos))
						may_go_backward = false;
					else
						may_go_forward = false;
				}
				if (may_go_backward) {
					b.pull = to_segm;
					--b.i;
					if (a_backward <= 1)
						poles[i] = pole_backward;
					else
						poles[i] = real_point_type(polyline[b.i]) + real_type(0.5) * d_backward;
				}
				else if (may_go_forward) {
					b.pull = to_segm;
					if (a_forward <= 1)
						poles[i] = pole_forward;
					else
						poles[i] = real_point_type(polyline[b.i]) + real_type(0.5) * d_forward;
				}
			}
			else {
				assert (b.pull == to_segm);
				real_type a;
				real_point_type p = polyline[b.i];
				real_point_type d = real_point_type(polyline[b.i+1]) - real_point_type(polyline[b.i]);
				if (intersect(a, p, d, pos, norm)) {
					if (a <= 0) {
						b.pull = to_node;
						poles[i] = p;
					} else if (a >= 1) {
						b.pull = to_node;
						++b.i;
						poles[i] = p + d;
					} else
						poles[i] = p + a * d;
				}
			}
		}

		++total_iterations_;

		if (precision_reached())
			return controls_;
	}
	return controls_;
}

template <typename T, typename R>
typename bezier_builder<T, R>::bezier_type const & bezier_builder<T, R>::build_arkan(nodes_type const & polyline)
{
	controls<R> ctrls;
	ctrls.push_back(controls_);
	std::vector<junc_type> junc(2, junc_cusp);
	real_point_vector_type attractors = ctrls;
	real_vector_type tractions(4);
	tractions[0] = tractions[3] = 1;
	tractions[1] = tractions[2] = 0;

	//fast_bspline_adjust requires that the distance between following points in polyline be equal to 1
	nodes_type polyline2;
	for (uint i = 0; i+1 < polyline.size(); ++i) {
		node_type curr = polyline[i];
		node_type target = polyline[i+1];
		do {
			polyline2.push_back(curr);
			//Brezenham algorithm will be preferred here
			if (curr.x < target.x)
				++curr.x;
			else if (curr.x > target.x)
				--curr.x;
			else if (curr.y < target.y)
				++curr.y;
			else if (curr.y > target.y)
				--curr.y;
		} while (curr != target);
	}
	polyline2.push_back(polyline.back());

	chisq_ = fast_bspline_adjust(
		polyline2,
		fast_bspline_adjust_params(
			max_iterations,
			length(polyline2) / polyline.size(),
			fit_local,
			translation, //is not really used
			false, //antiloop
			100),
		ctrls,
		junc,
		&poles, 
		&samples,
		&attractors,
		&tractions
	);

	total_iterations_ += max_iterations;
	controls_ = ctrls.piece(0);
	return controls_;
}

template bezier_builder<unsigned short>;
template bezier_builder<short>;
template bezier_builder<int>;
template bezier_builder<float>;
template bezier_builder<double>;

} //namespace outline
